<!-- saved from url=(0014)about:internet -->
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1"> 1</a></span><span class="line"><span class="keyword">function</span> [ <span class="var type1" id="S2T9U3">h</span> <span class="var type1" id="S3T9U4">v</span> <span class="var type1" id="S4T9U5">a</span> ] = nav_h_ekf(<span class="var type1" id="S5T2U8">Q</span>, <span class="var type1" id="S6T13U9">H</span>, <span class="var type1" id="S7T15U10">R</span> ,<span class="var type1" id="S8T2U11">F</span>, <span class="var type1" id="S9T2U12">I</span> ,<span class="var type1" id="S10T27U13">Z</span>,<span class="var type1" id="S11T12U14">up</span> )</span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">%作者 ytf</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3"> 3</a></span><span class="line"><span class="comment">%日期 2017年4月27日</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4"> 4</a></span><span class="line"><span class="comment">%状态转移协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">%H 预测矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">%R 观测噪声方差</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%F 状态转移矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%I</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%Z 测量矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10">10</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11">11</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S12T3U16">X</span> ;<span class="comment">% 状态变量</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12">12</a></span><span class="line">          <span class="keyword">if</span>(<span class="mxinfo" id="T1:U12"><span class="mxinfo" id="T1:U13">isempty(<span class="var type0" id="S12T0U22">X</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13">13</a></span><span class="line">           <span class="mxinfo" id="T3:U14"><span class="var type1" id="S12T3U25">X</span>=<span class="mxinfo" id="T3:U16">single(<span class="mxinfo" id="T34:U17">[0 0 0 ]'</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14">14</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15">15</a></span><span class="line">          </span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16">16</a></span><span class="line"> <span class="keyword">persistent</span> <span class="var type1" id="S15T2U35">P</span> ;<span class="comment">% 状态协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17">17</a></span><span class="line">            <span class="keyword">if</span>(<span class="mxinfo" id="T1:U19"><span class="mxinfo" id="T1:U20">isempty(<span class="var type0" id="S15T0U41">P</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18">18</a></span><span class="line">            <span class="mxinfo" id="T2:U21"><span class="var type1" id="S15T2U44">P</span>=<span class="mxinfo" id="T2:U23">single(<span class="mxinfo" id="T35:U24">[100 0 0 ;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19">19</a></span><span class="line"><span class="mxinfo" id="T2:U21"><span class="mxinfo" id="T2:U23"><span class="mxinfo" id="T35:U24">                      0 100 0 ;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20">20</a></span><span class="line"><span class="mxinfo" id="T2:U21"><span class="mxinfo" id="T2:U23"><span class="mxinfo" id="T35:U24">                      0 0 100 ;]</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21">21</a></span><span class="line">            <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22">22</a></span><span class="line">       <span class="mxinfo" id="T3:U25"><span class="var type1" id="S12T3U62">X</span>=<span class="mxinfo" id="T3:U27"><span class="var type1" id="S8T2U64">F</span>*<span class="var type1" id="S12T3U65">X</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23">23</a></span><span class="line">       <span class="mxinfo" id="T2:U30"><span class="var type1" id="S15T2U68">P</span>=<span class="mxinfo" id="T2:U32"><span class="mxinfo" id="T2:U33"><span class="mxinfo" id="T2:U34"><span class="var type1" id="S8T2U72">F</span>*<span class="var type1" id="S15T2U73">P</span></span>*<span class="mxinfo" id="T2:U37"><span class="var type1" id="S8T2U75">F</span>'</span></span>+<span class="var type1" id="S5T2U76">Q</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24">24</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U40"><span class="var type1" id="S11T12U80">up</span>==<span class="mxinfo" id="T6:U42">1</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25">25</a></span><span class="line">       <span class="mxinfo" id="T14:U43"><span class="var type1" id="S16T14U84">K</span>=<span class="mxinfo" id="T14:U45"><span class="mxinfo" id="T14:U46"><span class="var type1" id="S15T2U87">P</span>*<span class="mxinfo" id="T14:U48"><span class="var type1" id="S6T13U89">H</span>'</span></span>*<span class="mxinfo" id="T15:U50">pinv(<span class="mxinfo" id="T15:U51"><span class="mxinfo" id="T15:U52"><span class="mxinfo" id="T13:U53"><span class="var type1" id="S6T13U95">H</span>*<span class="var type1" id="S15T2U96">P</span></span>*<span class="mxinfo" id="T14:U56"><span class="var type1" id="S6T13U98">H</span>'</span></span> +<span class="var type1" id="S7T15U99">R</span></span>)</span></span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26">26</a></span><span class="line">       <span class="mxinfo" id="T3:U59"><span class="var type1" id="S12T3U102">X</span>=<span class="mxinfo" id="T3:U61"><span class="var type1" id="S12T3U104">X</span>+<span class="mxinfo" id="T3:U63"><span class="var type1" id="S16T14U106">K</span>*(<span class="mxinfo" id="T27:U65"><span class="var type1" id="S10T27U109">Z</span>-<span class="mxinfo" id="T27:U67"><span class="var type1" id="S6T13U111">H</span>*<span class="var type1" id="S12T3U112">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27">27</a></span><span class="line">       <span class="mxinfo" id="T2:U70"><span class="var type1" id="S15T2U115">P</span>=<span class="mxinfo" id="T2:U72">(<span class="mxinfo" id="T2:U73">single(<span class="var type1" id="S9T2U121">I</span>)-<span class="mxinfo" id="T2:U75"><span class="var type1" id="S16T14U123">K</span>*<span class="var type1" id="S6T13U124">H</span></span></span>)*<span class="var type1" id="S15T2U125">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28">28</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29">29</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30">30</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U79"><span class="var type1" id="S11T12U129">up</span>==<span class="mxinfo" id="T6:U81">0</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31">31</a></span><span class="line">       <span class="mxinfo" id="T33:U82"><span class="var type1" id="S18T33U133">H_H</span>=<span class="mxinfo" id="T33:U84"><span class="var type1" id="S6T13U135">H</span>(<span class="mxinfo" id="T6:U86">2</span>,:)</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,32" id="srcline32">32</a></span><span class="line">       <span class="mxinfo" id="T3:U87"><span class="var type1" id="S16T3U140">K</span>=<span class="mxinfo" id="T3:U89"><span class="mxinfo" id="T3:U90"><span class="var type1" id="S15T2U143">P</span>*<span class="mxinfo" id="T3:U92"><span class="var type1" id="S18T33U145">H_H</span>'</span></span>*<span class="mxinfo" id="T9:U94">pinv(<span class="mxinfo" id="T9:U95"><span class="mxinfo" id="T9:U96"><span class="mxinfo" id="T33:U97"><span class="var type1" id="S18T33U151">H_H</span>*<span class="var type1" id="S15T2U152">P</span></span>*<span class="mxinfo" id="T3:U100"><span class="var type1" id="S18T33U154">H_H</span>'</span></span> +<span class="mxinfo" id="T9:U102"><span class="var type1" id="S7T15U156">R</span>(<span class="mxinfo" id="T6:U104">2</span>,<span class="mxinfo" id="T6:U105">2</span>)</span></span>)</span></span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,33" id="srcline33">33</a></span><span class="line">       <span class="mxinfo" id="T3:U106"><span class="var type1" id="S12T3U161">X</span>=<span class="mxinfo" id="T3:U108"><span class="var type1" id="S12T3U163">X</span>+<span class="mxinfo" id="T3:U110"><span class="var type1" id="S16T3U165">K</span>*(<span class="mxinfo" id="T9:U112"><span class="mxinfo" id="T9:U113"><span class="var type1" id="S10T27U169">Z</span>(<span class="mxinfo" id="T6:U115">2</span>)</span>-<span class="mxinfo" id="T9:U116"><span class="var type1" id="S18T33U172">H_H</span>*<span class="var type1" id="S12T3U173">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,34" id="srcline34">34</a></span><span class="line">       <span class="mxinfo" id="T2:U119"><span class="var type1" id="S15T2U176">P</span>=<span class="mxinfo" id="T2:U121">(<span class="mxinfo" id="T2:U122">single(<span class="var type1" id="S9T2U182">I</span>)-<span class="mxinfo" id="T2:U124"><span class="var type1" id="S16T3U184">K</span>*<span class="var type1" id="S18T33U185">H_H</span></span></span>)*<span class="var type1" id="S15T2U186">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,35" id="srcline35">35</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,36" id="srcline36">36</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,37" id="srcline37">37</a></span><span class="line">       <span class="mxinfo" id="T9:U128"><span class="var type1" id="S2T9U189">h</span>=<span class="mxinfo" id="T9:U130"><span class="var type1" id="S12T3U191">X</span>(<span class="mxinfo" id="T6:U132">1</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,38" id="srcline38">38</a></span><span class="line">       <span class="mxinfo" id="T9:U133"><span class="var type1" id="S3T9U195">v</span>=<span class="mxinfo" id="T9:U135"><span class="var type1" id="S12T3U197">X</span>(<span class="mxinfo" id="T6:U137">2</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,39" id="srcline39">39</a></span><span class="line">       <span class="mxinfo" id="T9:U138"><span class="var type1" id="S4T9U201">a</span>=<span class="mxinfo" id="T9:U140"><span class="var type1" id="S12T3U203">X</span>(<span class="mxinfo" id="T6:U142">3</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,40" id="srcline40">40</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,41" id="srcline41">41</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,42" id="srcline42">42</a></span><span class="line">       </span></span>
</pre>
